// Author: 	Asil Kaan Bozcuoglu
// Date: 	Apr 25 2011
// From raw data file to pcd file.

#include <iostream>
#include "fstream"
#include "sstream"
#include "pcl/io/pcd_io.h"
#include "pcl/point_types.h"
#include "pcl_ros/io/bag_io.h"

#include "sys/stat.h"
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <pcl_ros/transforms.h>
#define MAX_SAMPLE_INDEX 10000

using namespace pcl;
using namespace pcl::io;
using namespace std;


int main(int argc, char* argv[])
{
	string inPath;
	string outPath;
	
	cout	<<"Enter the path of the input cloud"<<endl;
    	cin>>inPath;

	cout	<<"Enter the path of the pcd file"<<endl;
    	cin>>outPath;

	ifstream inFile;
	inFile.open(inPath.c_str());

	pcl::PointCloud<pcl::PointXYZ> cloud;
	cloud.header.frame_id = "/";
	cloud.width    = 176;
  	cloud.height   = 144;
  	cloud.is_dense = false;
  	cloud.points.resize (176 * 144);
	for(int j=0;j<144*176;j++)
	{
		double x, y, z;

		inFile >> x;
		inFile >> y;
		inFile >> z;		
		
		cloud.points[j].x = x / 65536.0;
		cloud.points[j].y = y / 65536.0;
		cloud.points[j].z = z / 65536.0; 
	}
	pcl::PCDWriter writer;
	writer.write(outPath, cloud, false); 

	inFile.close();

	return 0;
}
